capture_base.h

Go to the documentation of this file.

// WOLF - Copyright (C) 2020-2026
// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
// Joan Vallvé Navarro (jvallve@iri.upc.edu)
// All rights reserved.
//
// This file is part of WOLF: http://www.iri.upc.edu/wolf
// WOLF is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License version 3
// as published by the Free Software Foundation.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#pragma once
 
// Forward declarations for node templates
namespace wolf
{
class FrameBase;
class FeatureBase;
}  // namespace wolf
 
// Wolf includes
#include "core/common/wolf.h"
#include "core/common/node_state_blocks.h"
#include "core/common/time_stamp.h"
 
// std includes
 
namespace wolf
{
// class CaptureBase
class CaptureBase : public NodeStateBlocks
{
    friend class FeatureBase;
    friend class FactorBase;
    friend class FrameBase;
 
  private:
    FrameBaseWPtr      frame_ptr_;
    FeatureBasePtrList feature_list_;
    SensorBaseWPtr     sensor_ptr_;  
 
    static unsigned int capture_id_count_;
 
  protected:
    unsigned int capture_id_;
    TimeStamp    time_stamp_;  
    void         setProblem(ProblemPtr _problem) override final;
 
  public:
    CaptureBase(const std::string&     _type,
                const TimeStamp&       _ts,
                SensorBasePtr          _sensor_ptr    = nullptr,
                const TypeComposite&   _state_types   = {},
                const VectorComposite& _state_vectors = {},
                const PriorComposite&  _state_priors  = {});
 
    ~CaptureBase() override = default;
 
    void remove(bool viral_remove_parent_without_children = false) override;
 
    bool hasChildren() const override;
 
    virtual FactorBasePtr emplaceDriftFactor(CaptureBasePtr _capture_origin, char _key, bool _apply_loss_function);
 
    virtual bool isMotion() const;
 
    bool process();
 
    unsigned int id() const override;
    TimeStamp    getTimeStamp() const;
    void         setTimeStamp(const TimeStamp& _ts);
    void         setTimeStampToNow();
 
    FrameBaseConstPtr getFrame() const;
    FrameBasePtr      getFrame();
 
  private:
    void setFrame(const FrameBasePtr _frm_ptr);
 
  public:
    FeatureBaseConstPtrList getFeatureList() const;
    FeatureBasePtrList      getFeatureList();
    bool                    hasFeature(const FeatureBaseConstPtr& _feature) const;
 
    FactorBaseConstPtrList getFactorList() const;
    FactorBasePtrList      getFactorList();
    void                   getFactorList(FactorBaseConstPtrList& _fac_list) const;
    void                   getFactorList(FactorBasePtrList& _fac_list);
 
    SensorBaseConstPtr getSensor() const;
    SensorBasePtr      getSensor();
    virtual void       setSensor(const SensorBasePtr sensor_ptr);
 
  public:
    // State blocks
    StateBlockConstPtr getStateBlock(const char& _key) const override;
    StateBlockPtr      getStateBlock(const char& _key) override;
    StateBlockConstPtr getSensorP() const;
    StateBlockPtr      getSensorP();
    StateBlockConstPtr getSensorO() const;
    StateBlockPtr      getSensorO();
    StateBlockConstPtr getSensorIntrinsic() const;
    StateBlockPtr      getSensorIntrinsic();
 
    void fix() override;
    void unfix() override;
 
    void link(FrameBasePtr _frm_ptr);
 
    template <typename classType, typename... T>
    static std::shared_ptr<classType> emplace(FrameBasePtr _frm_ptr, T&&... all);
 
    virtual void printHeader(int           depth,
                             bool          factored_by,
                             bool          metric,
                             bool          state_blocks,
                             std::ostream& stream,
                             std::string   _tabs = "") const;
 
    void print(int           depth,
               bool          factored_by,
               bool          metric,
               bool          state_blocks,
               std::ostream& stream = std::cout,
               std::string   _tabs  = "") const;
 
    virtual CheckLog localCheck(bool _verbose, std::ostream& _stream, std::string _tabs = "") const;
 
    bool check(CheckLog& _log, bool _verbose, std::ostream& _stream, std::string _tabs = "") const;
 
  private:
    FeatureBasePtr addFeature(FeatureBasePtr _ft_ptr);
    void           removeFeature(FeatureBasePtr _ft_ptr);
 
  public:
    CaptureBasePtr      shared_from_this_capture();
    CaptureBaseConstPtr shared_from_this_capture() const;
};
 
}  // namespace wolf
 
#include "core/sensor/sensor_base.h"
#include "core/frame/frame_base.h"
#include "core/feature/feature_base.h"
#include "core/state_block/state_block.h"
 
namespace wolf
{
inline bool CaptureBase::isMotion() const
{
    return false;
}
 
template <typename classType, typename... T>
std::shared_ptr<classType> CaptureBase::emplace(FrameBasePtr _frm_ptr, T&&... all)
{
    std::shared_ptr<classType> cpt = std::make_shared<classType>(std::forward<T>(all)...);
    cpt->emplacePriors();
    cpt->link(_frm_ptr);
    return cpt;
}
 
inline StateBlockConstPtr CaptureBase::getSensorP() const
{
    return getStateBlock('P');
}
 
inline StateBlockPtr CaptureBase::getSensorP()
{
    return getStateBlock('P');
}
 
inline StateBlockConstPtr CaptureBase::getSensorO() const
{
    return getStateBlock('O');
}
 
inline StateBlockPtr CaptureBase::getSensorO()
{
    return getStateBlock('O');
}
 
inline StateBlockConstPtr CaptureBase::getSensorIntrinsic() const
{
    return getStateBlock('I');
}
 
inline StateBlockPtr CaptureBase::getSensorIntrinsic()
{
    return getStateBlock('I');
}
 
inline unsigned int CaptureBase::id() const
{
    return capture_id_;
}
 
inline FrameBaseConstPtr CaptureBase::getFrame() const
{
    if (frame_ptr_.expired())
        return nullptr;
    else
        return frame_ptr_.lock();
}
 
inline FrameBasePtr CaptureBase::getFrame()
{
    if (frame_ptr_.expired())
        return nullptr;
    else
        return frame_ptr_.lock();
}
 
inline void CaptureBase::setFrame(const FrameBasePtr _frm_ptr)
{
    frame_ptr_ = _frm_ptr;
}
 
inline FeatureBaseConstPtrList CaptureBase::getFeatureList() const
{
    FeatureBaseConstPtrList list_const;
    for (auto&& obj_ptr : feature_list_) list_const.push_back(obj_ptr);
    return list_const;
}
 
inline FeatureBasePtrList CaptureBase::getFeatureList()
{
    return feature_list_;
}
 
inline TimeStamp CaptureBase::getTimeStamp() const
{
    return time_stamp_;
}
 
inline SensorBaseConstPtr CaptureBase::getSensor() const
{
    return sensor_ptr_.lock();
}
 
inline SensorBasePtr CaptureBase::getSensor()
{
    return sensor_ptr_.lock();
}
 
inline void CaptureBase::setSensor(const SensorBasePtr sensor_ptr)
{
    sensor_ptr_ = sensor_ptr;
}
 
inline void CaptureBase::setTimeStamp(const TimeStamp& _ts)
{
    time_stamp_ = _ts;
}
 
inline void CaptureBase::setTimeStampToNow()
{
    time_stamp_.setToNow();
}
 
inline CaptureBasePtr CaptureBase::shared_from_this_capture()
{
    return std::static_pointer_cast<CaptureBase>(shared_from_this());
}
 
inline CaptureBaseConstPtr CaptureBase::shared_from_this_capture() const
{
    return std::static_pointer_cast<const CaptureBase>(shared_from_this());
}
 
}  // namespace wolf